#ifndef MOTOR_IDENTIFY_H
#define MOTOR_IDENTIFY_H

#include "motor_driver.h"
#include "current_ctl_loop.h"
#include "arm_math.h"
#include "adc.h"
#include "tim.h"
#include "Encoder.h"

// ------------------- 电阻辨识状态定义 -------------------
typedef enum
{
    RESISTANCE_IDLE,      // 空闲
    RESISTANCE_STAGE1,        // 0.5倍额定电流注入
    RESISTANCE_STAGE2,        // 0.8倍额定电流注入
    RESISTANCE_CALCULATE,     // 计算电阻
    RESISTANCE_FINISH         // 辨识完成
} ResistanceIdentifyState;


typedef enum {
    HF_IDLE,
    HF_INJECTION,          // 高频电压注入
    HF_RESPONSE_CAPTURE,   // 电流响应采集
    HF_INDUCTANCE_CALC     // 电感计算
} HighFreqInjectionState;

// ------------------- 磁链辨识状态定义 -------------------
typedef enum {
    FLUX_IDLE = 0,      // 空闲
    FLUX_STAGE1,        // 阶段1：稳定在 i_d1
    FLUX_STAGE2,        // 阶段2：稳定在 i_d2
    FLUX_CALCULATE,     // 计算磁链
    FLUX_FINISH         // 完成
} FluxIdentifyState;

// ------------------- 电阻辨识控制结构体 -------------------
typedef struct
{
    ResistanceIdentifyState state;    // 当前辨识状态
    float                    id_ref[2];// 0.5/0.8倍额定电流参考
    float                    i_d[2];   // 采集的d轴电流
    float                    u_d[2];   // 采集的d轴电压
    uint32_t                 sample_cnt;// 采样计数器
    uint32_t                 stable_cnt;// 稳定判定计数器
    float                    r_identified;// 辨识电阻
    bool                     start_flag;// 启动标志
    float                    rated_current;// 电机额定电流
} ResistanceIdentifyCtrl;

// ------------------- 阶跃电压法电感辨识状态定义 -------------------
typedef enum {
    STEP_INDUCTANCE_IDLE = 0,        // 空闲
    STEP_INDUCTANCE_D_INJECT,     // 阶跃电压注入
    STEP_INDUCTANCE_D_CAPTURE,    // 电流响应采集
    STEP_INDUCTANCE_CALCULATE_D,  // 电感计算
	
    STEP_INDUCTANCE_FINISH      // 辨识完成
} StepInductanceState;


// ------------------- 阶跃电压法电感辨识控制结构体 -------------------
typedef struct {
    StepInductanceState state;      // 当前辨识状态
    float step_voltage;             // 阶跃电压幅值 (V)，可配置
    float rated_current;            // 电机额定电流 (A)，用于稳态电流计算
    float sample_freq;              // 采样频率 (Hz)，用于时间计算
    float i_sample_buf[512];        // 电流采样缓存，按需调整长度
    uint16_t sample_idx;            // 采样缓存索引
    float L_identified;             // 辨识得到的电感值 (H)
    bool start_flag;                // 启动标志
    // 内部计算辅助
    float I_inf;                    // 稳态电流理论值 (I_inf = U/R，R 需提前或同步辨识)
    float tau;                      // 时间常数 (tau = L/R)
    uint32_t capture_cnt;           // 采集计数器
	    
	
	// 新增d-q轴相关参数
    float ud_step;            // d轴阶跃电压

    float Ld_identified;      // d轴电感

	
} StepInductanceIdentifyCtrl;



// ------------------- 高频注入电感辨识控制结构体 -------------------
typedef struct {
    float Vh;               // 高频电压幅值 (V)
    float wh;               // 高频电压角频率 (rad/s)
    float Ts;               // 注入信号周期 (s)
    HighFreqInjectionState state;          // 辨识阶段
    float u_dh, u_qh;       // dq轴高频注入电压
    float i_dh_buf[256], i_qh_buf[256]; // 电流响应缓存
    uint16_t inj_idx;       // 缓存索引
    uint16_t cap_idx;       // 缓存索引
    float L_identified;     // 辨识同步电感 L_s（替代原 Ld/Lq）
    bool  start_flag;       // 启动标志
    // 滤波后电流
    float i_d_bp;
    float i_q_bp;
    // 新增：异常值滤波
    float i_d_filtered;
    float i_q_filtered;
    float i_d_prev[3];
    float i_q_prev[3];
    uint8_t filter_idx;
} HighFreqInjectionCtrl;



// ------------------- 磁链辨识控制结构体 -------------------
typedef struct {
    FluxIdentifyState state;
    bool start_flag;

    float psi_f;        // 辨识得到的永磁磁链
    float we;           // 电机电角速度（需外部传入或测量）
    // 阶段1数据
    float i_d1;         // 第1组d轴电流指令
    float u_q1;         // 第1组q轴电压反馈
    // 阶段2数据
    float i_d2;         // 第2组d轴电流指令
    float u_q2;         // 第2组q轴电压反馈

    // 稳定判定参数
    uint32_t stable_cnt; 
    const uint32_t STABLE_THRESHOLD; // 电流稳定次数阈值
} FluxIdentifyCtrl;


//二阶IIR带通滤波器
typedef struct{
	float b0,b1,b2;
	float a1,a2;
	float x1,x2;
	float y1,y2;
} BiquadFilter;





extern ResistanceIdentifyCtrl g_res_identify;
extern StepInductanceIdentifyCtrl g_step_inductance_identify;
extern HighFreqInjectionCtrl g_hf_inj;
extern FluxIdentifyCtrl g_flux_identify;


// ------------------- 阶跃电压法电感辨识函数声明 -------------------
void StepInductanceIdentifyInit(void);
void StepInductanceIdentifyProcess(void);

void StepInductanceIdentifyStart(void);

// ------------------- 初始化与控制函数声明 -------------------
void MotorIdentifyInit(void);
void ResistanceIdentifyProcess(void);
void HighFreqInductanceProcess(void);
void FluxIdentifyProcess(void);
void ResistanceIdentifyStart(void);
void HighFreqInductanceStart(void);
void MotorIdentifyPeriodicTask(void);

// ------------------- 内部辅助函数声明（若需对外暴露可调整） -------------------
void HFVoltageInjectionGen(void);
void HFResponseExtract(float i_d, float i_q);

// ------------------- 滤波器函数声明 -------------------
void InitBiquadFilter(BiquadFilter *filter, float center_freq, float sample_freq, float q_factor);
float ApplyBiquadFilter(BiquadFilter *filter, float input);
float LimitValue(float value, float min, float max);
float MedianFilter(float new_value, float prev_values[3], uint8_t *idx);
float MovingAverageFilter(float new_value, float *prev_value);
#endif /* MOTOR_IDENTIFY_H */